/*
 * ultrasonic.c
 *
 *  Created on: Apr 26, 2013
 *      Author: Daniel
 */

#include "DSP28x_Project.h"
#include "imu.h"

void InitUltrasonic(void) {
	// setup SOC0-SOC4
	// SOC0 will be a throwaway sample (suggested by E2E answer)
	// SOC1-4 will be for oversampling of the signal
	AdcRegs.ADCSOC0CTL.bit.ACQPS = 6;	// sets acquisition window size
	AdcRegs.ADCSOC1CTL.bit.ACQPS = 6;	// width of window is ACQPS + 1
	AdcRegs.ADCSOC2CTL.bit.ACQPS = 6;	// times the ADCLK period
	AdcRegs.ADCSOC3CTL.bit.ACQPS = 6;
	AdcRegs.ADCSOC4CTL.bit.ACQPS = 6;

	AdcRegs.ADCSOC0CTL.bit.CHSEL = 0x0;	// sample ADCINA0
	AdcRegs.ADCSOC1CTL.bit.CHSEL = 0x0;
	AdcRegs.ADCSOC2CTL.bit.CHSEL = 0x0;
	AdcRegs.ADCSOC3CTL.bit.CHSEL = 0x0;
	AdcRegs.ADCSOC4CTL.bit.CHSEL = 0x0;

	return;
}

Uint16 sampleUltrasonic(void) {
	int i;
	AdcRegs.ADCSOCFRC1.all = 0x001F;	// enable sampling of SOCs 1-4
	for(i = 0; i < 500; i++);			// wait for conversion to complete
	return (AdcResult.ADCRESULT1 + AdcResult.ADCRESULT2 + AdcResult.ADCRESULT3 + AdcResult.ADCRESULT4)/4 >> 3;
	// return an average of the four measurements, bit shifted by 3
}
